#include "HoughDoorDetector.hh"
#include "ConfigFile.h"
#include <cmath>
#include <iostream>
#include <deque>

using namespace std;

HoughDoorDetector::HoughDoorDetector(std::string configFilename) : hough_(0)
{
  util::ConfigFile conf(configFilename);

  // smoothVel()'s parameters
  DOOR_HOUGH_THETA_STEP = conf.getDouble("DOOR_HOUGH_THETA_STEP", M_PI*2.0/180.0); // rad
  DOOR_HOUGH_RHO_STEP = conf.getDouble("DOOR_HOUGH_RHO_STEP", 0.05);  // m
  DOOR_HOUGH_RHO_MAX = conf.getDouble("DOOR_HOUGH_RHO_MAX", 8.00);  // m
  DOOR_MIN_WALL_DIST = conf.getDouble("DOOR_MIN_WALL_DIST", 0.10);  // m
  DOOR_MAX_WALL_RECESS = conf.getDouble("DOOR_MAX_WALL_RECESS", 0.50);  // m
  DOOR_SIDE_WALL_NUM = conf.getInt("DOOR_SIDE_WALL_NUM", 2);  // m
  DOOR_WIDTH_MEAN = conf.getDouble("DOOR_WIDTH_MEAN", 0.90);  // m
  DOOR_WIDTH_TOLL = conf.getDouble("DOOR_WIDTH_TOLL", 0.10);  // m

  hough_ = new HoughSpectrum(DOOR_HOUGH_THETA_STEP,
                             DOOR_HOUGH_RHO_STEP,DOOR_HOUGH_RHO_MAX);
}

HoughDoorDetector::~HoughDoorDetector()
{
  delete hough_;
}

void HoughDoorDetector::findDoors(const line::point_vector& scan,door_vector& doors)
{
  // Computes Hough TRansform and Hough Spectrum
  hough_->compute(scan);

  // Returns the maximum of Hough Spectrum
  double thetaMax = hough_->spectrumMax();
  unsigned int ithetaMax = hough_->spectrumMaxIdx();
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
  cerr << __FILE__ << "," << __LINE__ << ": max Hough Spectrum " 
       << " at index " << ithetaMax << ", angle " << thetaMax << " rad = "
       << 180.0*thetaMax*M_1_PI << " deg"<< endl;
#endif
  
  // Given thetaMax, computes rho_i that maximize HT(thetaMax,rho_i).
  // Such values corresponds to the candidate wall distances from the robot. 
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
  cerr << __FILE__ << "," << __LINE__ << ": candidate local maxima: " << endl;
#endif
  std::vector<HistValue> localMaxima;
  unsigned int rhoNum = hough_->rhoBinNum();
  for (unsigned int i = 1; i < rhoNum-1; ++i) {
    double value = hough_->idxToRho(i);
    unsigned int hist = hough_->houghValue(ithetaMax,i);
    unsigned int histPrev = hough_->houghValue(ithetaMax,i-1);
    unsigned int histNext = hough_->houghValue(ithetaMax,i-1);
    if (hist > histPrev && hist > histNext) {
      localMaxima.push_back(HistValue(i,value,hist));
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
      cerr << " max at " << value << ", index " << i << ", hist " << hist << endl;
#endif
    }
  }
  // Local maxima are filtered: we require that the distance between two 
  // peaks is at least equal to DOOR_MIN_WALL_DIST
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
  cerr << __FILE__ << "," << __LINE__ << ": selected local maxima: " << endl;
#endif
  std::greater<HistValue> op;
  std::sort(localMaxima.begin(),localMaxima.end(),op);
  std::deque<HistValue> peakMaxima;
  for (unsigned int i = 0; i < localMaxima.size(); ++i) {
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
    cerr << " evaluation max " << localMaxima[i].value << ", idx " 
         << localMaxima[i].idx << ", hist " << localMaxima[i].hist << endl;
#endif
    bool valid = true;
    for (unsigned int j = 0; j < peakMaxima.size(); ++j) {
      if (::fabs(localMaxima[i].value - peakMaxima[j].value) < DOOR_MIN_WALL_DIST) {
        valid = false;
        break;
      }
    }
    if (valid) {
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
      cerr << " peak at " << localMaxima[i].value << ", index " << localMaxima[i].idx 
           << ", hist " << localMaxima[i].hist << endl;
#endif
      peakMaxima.push_front(localMaxima[i]);
    }
  }
  // Extracts the largest peak with rho > 0 and the largest with rho < 0 
  // (left and right walls)
  double rhoMin = DOOR_HOUGH_RHO_MAX;
  double rhoMax = -DOOR_HOUGH_RHO_MAX;
  while (!peakMaxima.empty() && rhoMin > rhoMax) {
    if (peakMaxima.back().value < 0 && peakMaxima.back().value < rhoMin) {
      rhoMin = peakMaxima.back().value;
    }
    else if (peakMaxima.back().value > 0 && peakMaxima.back().value > rhoMax) {
      rhoMax = peakMaxima.back().value;
    }
    peakMaxima.pop_back();
  }
  if (rhoMin > 0) rhoMin = -2.0*DOOR_HOUGH_RHO_MAX;
  if (rhoMax < 0) rhoMin =  2.0*DOOR_HOUGH_RHO_MAX;
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
  cerr << __FILE__ << "," << __LINE__ << ": wall distances " << rhoMin << ","
       << rhoMax << endl;
#endif
  // Doors are detected when there is a point scan[i] so that
  // scan[i].x * cos(thetaMax) + scan[i].y * sin(thetaMax) < (rhoMin-DOOR_MIN_WALL_DIST)
  //     OR
  // scan[i].x * cos(thetaMax) + scan[i].y * sin(thetaMax) > (rhoMax+DOOR_MIN_WALL_DIST)
  door_type door;
  bool startDoor = false;
  double rhoDoor;
  for (unsigned int i = 0; i < scan.size(); ++i) {
    double distSide = scan[i].x * cos(thetaMax) + scan[i].y * sin(thetaMax);
    double distFront = -scan[i].x * sin(thetaMax) + scan[i].y * cos(thetaMax);
    bool underMin = distSide < (rhoMin-DOOR_MAX_WALL_RECESS);
    bool aboveMax = distSide > (rhoMax+DOOR_MAX_WALL_RECESS);
    bool inside = !underMin && !aboveMax;
    if (!startDoor && !inside) {
      startDoor =  true;
      door.first = (i > 0 ? scan[i-1] : scan[i]);
      rhoDoor = distSide*rhoMin > 0 ? rhoMin : rhoMax;
      if (i > 0) {
        computeProjection(scan[i-1],door.first,thetaMax,rhoDoor);
      }
      else {
        computeProjection(scan[i],door.first,thetaMax,rhoDoor);
      }
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
      cerr << __FILE__ << "," << __LINE__ << ": starting door at " << (i-1)
           << ", point " << door.first.x << "," << door.first.y << endl
           << "  angle " << 180.0*M_1_PI*atan2(scan[i].y,scan[i].x) << " deg, "
           << " point " << scan[i].x << "," << scan[i].y 
           << " distSide " << distSide << endl;
#endif
    }
    else if (startDoor && inside) {
      startDoor =  false;
      computeProjection(scan[i],door.second,thetaMax,rhoDoor);
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
      cerr << __FILE__ << "," << __LINE__ << ": stopping door at " << i 
           << ", point " << door.second.x << "," << door.second.y << endl
           << "  angle " << 180.0*M_1_PI*atan2(scan[i].y,scan[i].x) << " deg, "
           << " point " << scan[i].x << "," << scan[i].y 
           << " distSide " << distSide << endl;
#endif
      door.center.x = 0.5 * (door.first.x + door.second.x);
      door.center.y = 0.5 * (door.first.y + door.second.y);
      door.width = distance(door.first,door.second);
      door.thetaNorm = thetaMax;
      doors.push_back(door);
    }
  }
  if (startDoor) {
    startDoor = false;
    computeProjection(scan.back(),door.second,thetaMax,rhoDoor);
#ifdef HOUGH_DOOR_DETECTOR_DEBUG
      cerr << __FILE__ << "," << __LINE__ << ": stopping door at last"
           << " point " << door.second.x << "," << door.second.y << endl;
#endif
    door.center.x = 0.5 * (door.first.x + door.second.x);
    door.center.y = 0.5 * (door.first.y + door.second.y);
    door.width = distance(door.first,door.second);
    door.thetaNorm = thetaMax;
    doors.push_back(door);
  }
}

double HoughDoorDetector::distance(line::point_type p1,line::point_type p2)
{
  return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y));
}

void HoughDoorDetector::computeProjection(const line::point_type& p,line::point_type& q,
                                          double theta,double rho)
{
  double yl =-p.x * sin(theta) + p.y * cos(theta);
  q.x = rho * cos(theta) - yl * sin(theta);
  q.y = rho * sin(theta) + yl * cos(theta); 
}

